/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf.cpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月12日
  ******************************************************************************
  */
/* USER CODE END Header */
#include "ekf.hpp"

void EKF::Ekf_Init()
{
	Update = false;
	dt = 0.001f;
	g << 0.0f,0.0f,-9.87f;
	osDelay(150);

	/*求状态变量初始值*/
	//获取IMU+MAG数据
	xQueuePeek(queueGyrDat, &gyro, 0);					//从队列中获取陀螺仪数据
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据

	MatGyr << gyro.gyro[0],gyro.gyro[1],gyro.gyro[2];	//Unit:rad/s
	MatAcc << -acc.acc[0],-acc.acc[1],-acc.acc[2];			//Unit:m/s2
	MatMag << mag.mag[0],mag.mag[1],mag.mag[2];			//Unit:uT

	//Acc、Mag数据归一化
	MatAcc.normalize();
	MatMag.normalize();

	Mb = MatMag;

	Euler[0] = atan2f(MatAcc(1),MatAcc(2));
	Euler[1] = -asinf(MatAcc(0));

	float mag_calib[2];
	float sinR,cosR,sinP,cosP;
	sinR = sinf(Euler[0]);
	cosR = cosf(Euler[0]);
	sinP = sinf(Euler[1]);
	cosP = cosf(Euler[1]);

	mag_calib[0]=-MatMag(1)*cosR + MatMag(2)*sinR;//分子
	mag_calib[1]=MatMag(0)*cosP + MatMag(1)*sinP*sinR + MatMag(2)*sinP*cosR;//分母

	if (mag_calib[1] != 0)
	{
		Euler[2] = atan2f(mag_calib[0], mag_calib[1]);
	}
	else
	{
		if (mag_calib[0] < 0)
			Euler[2] = -M_PI/2;
		else
			Euler[2] = M_PI/2;
	}

	//欧拉角转旋转矩阵,再转四元数
	q_p = AngleAxisf(Euler(2),Vector3f::UnitZ())*AngleAxisf(Euler(1),Vector3f::UnitY())*AngleAxisf(Euler(0),Vector3f::UnitX());

	//四元数归一化
	q_p.normalize();

	Cn2b = q_p.toRotationMatrix().transpose();
	M = Cn2b.transpose()*Mb;


	//状态变量初值
	X_p << q_p.w(),q_p.x(),q_p.y(),q_p.z(),0.0f,0.0f,0.0f;

	P_p = 1e-7f*MatrixXf::Identity(6, 6);

	float Q1,Q2;
	Q1 = 5e-5f;	//陀螺仪噪声方差
	Q2 = 1e-3f;	//陀螺仪偏置噪声方差
	Q.Identity();
	for(uint8_t i=0;i<3;i++)
	{
		Q(i,i) = Q1;
		Q(i+3,i+3) = Q2;
	}


	float R1,R2;
	R1 = 5e0f;	//加速度计噪声
	R2 = 1e-1f; //磁力计噪声
	R.Identity();
	for(uint8_t i=0;i<3;i++)
	{
		R(i,i) = R1;
	}
	R(3,3) = R2;


	H.Zero();
	H(3,2) = 1.0f;

	F << MatrixXf::Zero(3, 3),-MatrixXf::Identity(3, 3),
		 MatrixXf::Zero(3, 3), MatrixXf::Zero(3, 3);

	G <<-MatrixXf::Identity(3, 3),MatrixXf::Zero(3, 3),
		 MatrixXf::Zero(3, 3)    ,MatrixXf::Zero(3, 3);


}

void EKF::Ekf_Update()
{
	getTimer_us(&startTimer);

	xQueuePeek(queueGyrDat, &gyro, 0);					//从队列中获取陀螺仪数据
	xQueuePeek(queueAccDat, &acc, 0);					//从队列中获取加速度计数据
	xQueuePeek(queueMagDat, &mag, 0);					//从队列中获取磁力计数据
	MatGyr << gyro.gyro[0],gyro.gyro[1],gyro.gyro[2];	//Unit:rad/s
	MatAcc << acc.acc[0],acc.acc[1],acc.acc[2];			//Unit:m/s2
	MatMag << mag.mag[0],mag.mag[1],mag.mag[2];			//Unit:uT

	MatMag.normalize();
	//计算kalman gain

	Matrix<float,3,3> gb_x;
	gb = Cn2b * g;
	gb_x << 0.0f,-gb(2),gb(1),
			gb(2),0.0f,-gb(0),
			-gb(1),gb(0),0.0f;
	H.block(0, 0, 3, 3) = gb_x;

	K = P_p*H.transpose()*(H*P_p*H.transpose()+R).inverse();

	//更新
	P_k = (MatrixXf::Identity(6, 6) - K*H)*P_p;

	float mag_calib[2];
	float sinR,cosR,sinP,cosP;
	sinR = sinf(Euler[0]);
	cosR = cosf(Euler[0]);
	sinP = sinf(Euler[1]);
	cosP = cosf(Euler[1]);

	mag_calib[0]=-MatMag(1)*cosR + MatMag(2)*sinR;//分子
	mag_calib[1]=MatMag(0)*cosP + MatMag(1)*sinP*sinR + MatMag(2)*sinP*cosR;//分母

	if (mag_calib[1] != 0)
	{
		Euler[2] = atan2f(mag_calib[0], mag_calib[1]);
	}
	else
	{
		if (mag_calib[0] < 0)
			Euler[2] = -M_PI/2;
		else
			Euler[2] = M_PI/2;
	}
	y << MatAcc(0),MatAcc(1),MatAcc(2),Euler[2];

	y_p(0) = gb(0);
	y_p(1) = gb(1);
	y_p(2) = gb(2);
	y_p(3) =atan2f(2.0f * q_p.x() * q_p.y() + 2.0f * q_p.w() * q_p.z(), -2.0f * q_p.y() * q_p.y() - 2.0f * q_p.z() * q_p.z() + 1.0f);

	Vector4f y_sub_y_p;
	y_sub_y_p = y-y_p;
	if(y_sub_y_p(3)>M_PI)		y_sub_y_p(3) -= 2*M_PI;
	if(y_sub_y_p(3)<-M_PI)	y_sub_y_p(3) += 2*M_PI;
	X_k_error = K*y_sub_y_p;

//	X_k_error = K*(y-y_p);

	q_k = q_p * AngleAxisf(X_k_error(2),Vector3f::UnitZ())*AngleAxisf(X_k_error(1),Vector3f::UnitY())*AngleAxisf(X_k_error(0),Vector3f::UnitX());

	q_k.normalize();		//归一化四元数

	X_k(0) = q_k.w();
	X_k(1) = q_k.x();
	X_k(2) = q_k.y();
	X_k(3) = q_k.z();
	for(uint8_t i=0;i<3;i++)
	{
		X_k(i+4) = X_p(i+4) + X_k_error(i+3);
	}
//	Cn2b = q_k.toRotationMatrix().transpose();

	Euler[0] =  atan2f(2.0f * q_k.y() * q_k.z() + 2.0f * q_k.w() * q_k.x(), -2.0f * q_k.x() * q_k.x() - 2.0f * q_k.y() * q_k.y() + 1.0f);
	Euler[1] =  asinf(fConstrain(-2.0f * q_k.x() * q_k.z() + 2.0f * q_k.w() * q_k.y(),-1.0f,1.0f));
	Euler[2] =  atan2f(2.0f * q_k.x() * q_k.y() + 2.0f * q_k.w() * q_k.z(), -2.0f * q_k.y() * q_k.y() - 2.0f * q_k.z() * q_k.z() + 1.0f);

	//传播
	w_k << MatGyr(0) - X_k(4),MatGyr(1) - X_k(5),MatGyr(2) - X_k(6);
	q_p = q_k * AngleAxisf(w_k(2)*dt,Vector3f::UnitZ())*AngleAxisf(w_k(1)*dt,Vector3f::UnitY())*AngleAxisf(w_k(0)*dt,Vector3f::UnitX());

	q_p.normalize();

	X_p << q_p.w(),q_p.x(),q_p.y(),q_p.z(),X_k(4),X_k(5),X_k(6);

	Cn2b = q_p.toRotationMatrix().transpose();

	Matrix<float,3,3> w_k_x;
	w_k_x << 0.0f,-w_k(2),w_k(1),
			w_k(2),0.0f,-w_k(0),
			-w_k(1),w_k(0),0.0f;

	F.block(0, 0, 3, 3) = -1.0f*w_k_x;

	P_p = P_k + (F*P_k + P_k*F.transpose() + G*Q*G.transpose())*dt;

	ekfAngUvw.Ang[0] = Euler(0);
	ekfAngUvw.Ang[1] = Euler(1);
	ekfAngUvw.Ang[2] = Euler(2);
	xQueueOverwrite(queueEkf,&ekfAngUvw);

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;
}

EKF ekf((char *)"EKF");
extern "C" void ekf_main(void *argument)
{
	ekf.Ekf_Init();
	osDelay(5);
	for(;;)
	{
		osSemaphoreAcquire(semEkf,0xffffffff);
			ekf.Ekf_Update();		//Ekf任务耗时大概
	}
}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
